//
// Created by user on 20-12-21.
//


#include <iostream>
#include <vector>
#include "math.h"
using namespace std;


void hi_Q_normalise(float q1[4], float q2[4])
{
    float Q_length;
    Q_length = q1[0] * q1[0] + q1[1] * q1[1] + q1[2] * q1[2] + q1[3] * q1[3];
    Q_length = sqrt(Q_length);
    if (Q_length < 0.0001)
    {
        q2[0] = q1[0];
        q2[1] = q1[1];
        q2[2] = q1[2];
        q2[3] = q1[3];
    }
    else
    {
        q2[0] = q1[0] / Q_length;
        q2[1] = q1[1] / Q_length;
        q2[2] = q1[2] / Q_length;
        q2[3] = q1[3] / Q_length;
    }
}

void hi_axisAngle2dq(float gyro[3], float dt, float dq[4])
{
    float normValues[3];

    // Calculate the angular speed of the sample
    float omegaMagnitude =
            sqrt(gyro[0] * gyro[0] +
                 gyro[1] * gyro[1] +
                 gyro[2] * gyro[2]);

    // Normalize the rotation vector if it's big enough to get the axis
    if (omegaMagnitude > 0.0001) {
        normValues[0] = gyro[0] / omegaMagnitude;
        normValues[1] = gyro[1] / omegaMagnitude;
        normValues[2] = gyro[2] / omegaMagnitude;
    }

    // Integrate around this axis with the angular speed by the timestep
    // in order to get a delta rotation from this sample over the timestep
    // We will convert this axis-angle representation of the delta rotation
    // into a quaternion before turning it into the rotation matrix.
    float thetaOverTwo = omegaMagnitude * dt / 2;
    float sinThetaOverTwo = sin(thetaOverTwo);
    float cosThetaOverTwo = cos(thetaOverTwo);
    dq[1] = sinThetaOverTwo * normValues[0];
    dq[2] = sinThetaOverTwo * normValues[1];
    dq[3] = sinThetaOverTwo * normValues[2];
    dq[0] = cosThetaOverTwo;
}

void hi_Q_mul(float q1[4], float q2[4], float q3[4])
{
    q3[1] = q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2] + q1[0] * q2[1];
    q3[2] = -q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1] + q1[0] * q2[2];
    q3[3] = q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0] + q1[0] * q2[3];
    q3[0] = -q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3] + q1[0] * q2[0];
}


void applyGyroIntegration(float gyro[3], float dt, float Q_imu[4])
{
    float dq[4];
    hi_axisAngle2dq(gyro, dt, dq);
    float tmp_q[4];
    hi_Q_mul(Q_imu, dq, tmp_q);
    hi_Q_normalise(tmp_q, Q_imu);
}